# Untitled - By: dell - 周一 6月 6 2022

import sensor, image, time
from pyb import UART
import ustruct

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(time = 2000)

clock = time.clock()
    #阈值

th0 = (18,30,36,50,20,32)
th1 = (22,43,37,56,13,40)
th2 = (11,26,23,42,10,30)

uart = UART(3,9600)
uart.init(9600,bits=8,parity=None,stop=1)

def send_data(rho,theta,mag,plxs):
    global uart
    data = ustruct.pack("<BhhhfB",
                        0X3A,
                        int(rho),
                        int(theta),
                        int(mag),
                        float(plxs),
                        0X5C)
    uart.write(data)
    #print(data)


while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8).binary([th0,th1,th2])

    #直方图数据
    plxs = img.get_histogram().bins()[-1]

    #线性回归
    lin = img.get_regression([(100,255)],roi = (0,40,160,80))
    #img.draw_rectangle((0,40,160,80),color = (255,0,0))
    if(lin):
        rho_err = abs(lin.rho()) - img.width()/2
        if(lin.theta()>90):
            theta_err = lin.theta()-180
        else:
            theta_err = lin.theta()
        #img.draw_line(lin.line(),color = (0,0,255))
        send_data(rho_err,theta_err,lin.magnitude(),plxs)
        #print('rho:',rho_err,'\ttheta:',theta_err,'\tmag:',lin.magnitude(),"\tpls",plxs)

    #print(clock.fps())

